#include <iostream>
#include <cstring>
#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <random>
#include <chrono>
#include <thread>

class Thruster2Controller {
    public:
    Thruster2Controller(uint8_t thrusterNum, const std::string& ifname = "vcan0") 
        : thrusterNum_(thrusterNum) {
        
        std::cout << thrusterNum << std::endl;

        // 初始化随机数生成器
        rng.seed(std::chrono::system_clock::now().time_since_epoch().count());
        
        // 创建socket
        if ((sock_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
            perror("Socket creation failed");
            exit(EXIT_FAILURE);
        }

        // 指定CAN接口
        strcpy(ifr_.ifr_name, ifname.c_str());
        ioctl(sock_, SIOCGIFINDEX, &ifr_);

        // 绑定socket到CAN接口
        addr_.can_family = AF_CAN;
        addr_.can_ifindex = ifr_.ifr_ifindex;
        if (bind(sock_, (struct sockaddr *)&addr_, sizeof(addr_)) < 0) {
            perror("Bind failed");
            close(sock_);
            exit(EXIT_FAILURE);
        }

        // 初始化CAN ID (格式: 320X1)
        control_id_ = (0x32000 | (thrusterNum_ << 4) | 0x1) ;
        control_id_ = (control_id_ & CAN_EFF_MASK) | CAN_EFF_FLAG;
        //std::cout <<std::hex<< control_id_ << std::endl;
    }

    // 生成随机转速 (99%: -100~100, 1%: 1000)，精度0.1rpm
    
    int16_t generateRandomSpeed() {
        speedSendCount_++;
        if (speedSendCount_ % 100 != 0) {  // 前99次
            std::uniform_int_distribution<int16_t> speed_dist(-1000, 1000); // -100.0~100.0
            int16_t raw = speed_dist(rng);
            return raw; // 实际值 = raw * 0.1 rpm
        } else {  // 第100次
            return 10000; // 1000.0 rpm
        }
    }

    // 生成随机间隔 (99%: 99-101ms, 1%: <80 or >120, 极少数>200ms)
    int generateRandomInterval() {
        std::uniform_int_distribution<int> percent_dist(0, 99);
        int percent = percent_dist(rng);
        
        if (percent < 99) {  // 99%概率: 99-101ms
            std::uniform_int_distribution<int> normal_dist(99, 101);
            return normal_dist(rng);
        } else {  // 1%概率
            std::uniform_int_distribution<int> extreme_dist(0, 9);
            if (extreme_dist(rng) == 0) {  // 10% of 1% = 0.1%概率 >200ms
                std::uniform_int_distribution<int> very_long_dist(201, 300);
                return very_long_dist(rng);
            } else {  // 90% of 1% = 0.9%概率 <80 or >120
                std::uniform_int_distribution<int> abnormal_dist(0, 1);
                if (abnormal_dist(rng)) {
                    std::uniform_int_distribution<int> short_dist(10, 79);
                    return short_dist(rng);
                } else {
                    std::uniform_int_distribution<int> long_dist(121, 200);
                    return long_dist(rng);
                }
            }
        }
    }

    void sendSpeedCommand(int16_t speed, bool faultClear = false) {
        struct can_frame frame;
        memset(&frame, 0, sizeof(frame));
        
        // 设置CAN ID (格式: 32111)
        frame.can_id = 0x32111;
        frame.can_dlc = 8;
        
        // 第0字节为随机数
        std::uniform_int_distribution<uint8_t> rand_dist(0, 255);
        frame.data[0] = rand_dist(rng);
        // 第4、5字节为转速
        frame.data[4] = speed & 0xFF;         // 低8位
        frame.data[5] = (speed >> 8) & 0xFF;  // 高8位
        // 其他字节保留为0
        
        if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) {
            perror("Write failed");
        } else {  
            std::cout << "Sent speed command: " << speed << " rpm" << std::hex << control_id_ << std::endl;
            std::cout << "Thruster " << static_cast<int>(thrusterNum_)
            <<  std::hex << " | Speed: " << speed <<" rpm | "
                  << "CAN ID: 0x" << std::hex << control_id_ << std::dec << std::endl;
        }
    }

    void run() {
        while (true) {
            // 生成随机转速
            int16_t speed = generateRandomSpeed();
            
            // 发送命令（故障位固定为0）
            sendSpeedCommand(speed, false);
            
            // 获取随机间隔并休眠
            int interval = generateRandomInterval();
            std::this_thread::sleep_for(std::chrono::milliseconds(interval));
            
            // 打印实际间隔
            std::cout << "Next message in " << interval << "ms" << std::endl;
        }
    }

private:
    uint8_t thrusterNum_;
    int sock_;
    struct sockaddr_can addr_;
    struct ifreq ifr_;
    std::default_random_engine rng;
    uint32_t control_id_;  // 存储计算后的CAN ID
    uint8_t speedSendCount_; //记录转速发送次数
};

int main() {
    // 不再需要参数，直接初始化
    try {
        uint8_t num = 0x11; // 固定为0x11，对应canID 0x32111
        Thruster2Controller controller(num);  // 显式传递参数
        
        std::cout << "Thruster " << std::hex << static_cast<int>(num) 
                  << " initialized (ID: 0x" << std::hex << (0x32000 | (num << 4) | 0x1)
                  << ")" << std::dec << std::endl;
        
        controller.run();
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return 1;
    }
    return 0;
}